// Devin Koepl, Kevin Kemper


#include <fca_controllers/fca_interface.h>

// Macros for
#define QUOTEME_(x) #x
#define QUOTEME(x) 	QUOTEME_(x)

//*****************************************************************************

// SHM interface to kernel.

static Shm * shm;

//static FILE *fp = NULL;
static int logging_flag = 0;
static int cut_flag = 0;

pthread_t datalogging_thread;


ControllerData global_data;

//*****************************************************************************

int main (int argc, char **argv)
{
	int i;


//	pthread_t datalogging_thread;
//	pthread_t msg_thread;


	Gtk::Main gtk(argc, argv);

	// Create the relative path to the Glade file.
	std::string glade_gui_path = std::string(argv[0]);

	glade_gui_path = glade_gui_path.substr(0, glade_gui_path.rfind("/bin"));
	glade_gui_path = glade_gui_path.append("/src/fca_gui.glade");

	//*************************************************************************

	// Connect to kernel's shm.

	if ( !( shm = ( Shm * )rt_shm_alloc( nam2num( SHM_NAME ), 0, USE_VMALLOC ) ) )
		ROS_ERROR( "rtai_malloc() data to user space failed (maybe /dev/rtai_shm is missing)!" );

	ROS_INFO( "Connected to SHM." );

	//*************************************************************************
	// init vars
	global_data.command					= CMD_DISABLE;
	global_data.controller_requested	= 0;

	//*************************************************************************
	// Create threads.

	ROS_INFO( "Creating threads." );

//	pthread_create( &msg_thread, NULL, msg_task, NULL );
	
//	pthread_create( &datalogging_thread, NULL, datalogging_task, NULL );

	//*************************************************************************
	// GUI

	Glib::RefPtr<Gtk::Builder> gui = Gtk::Builder::create();
	try
	{
		gui->add_from_file(glade_gui_path);
	}
	catch(const Glib::FileError& ex)
	{
		ROS_ERROR("File Error");
//			ROS_ERROR("FileError: %d", ex.what());
	}
	catch(const Gtk::BuilderError& ex)
	{
		ROS_ERROR("Builder Error");
//			ROS_ERROR("BuilderError: %d", ex.what());
	}

	// Grab pointers to GUI objects
	gui->get_widget("fca_window", window);
	if(!window)
	{
		ROS_ERROR("No FCA Window");
	}

	gui->get_widget("controller_notebook", controller_notebook);


	// controller specifics
	gui->get_widget("motor_torque_hscale", motor_torque_hscale);

	gui->get_widget("motor_position_hscale", motor_position_hscale);
	gui->get_widget("p_motor_position_hscale", p_motor_position_hscale);
	gui->get_widget("d_motor_position_hscale", d_motor_position_hscale);

	gui->get_widget("force_hscale",		force_hscale);
	gui->get_widget("pos_hscale",		pos_hscale);
	gui->get_widget("tp_hscale",		tp_hscale);
	gui->get_widget("p_force_hscale",	p_force_hscale);
	gui->get_widget("d_force_hscale",	d_force_hscale);
	gui->get_widget("f_force_hscale",	f_force_hscale);
	gui->get_widget("p_pos_hscale",		p_pos_hscale);
	gui->get_widget("d_pos_hscale",		d_pos_hscale);
	gui->get_widget("force_trq_lim_hscale",	force_trq_lim_hscale);
	gui->get_widget("cut_chkbox", 		cut_chkbox);

	gui->get_widget("trq_amp_hscale",	trq_amp_hscale);
	gui->get_widget("trq_freq_hscale",	trq_freq_hscale);
	gui->get_widget("trq_off_hscale",	trq_off_hscale);
	gui->get_widget("trq_k1_hscale",	trq_k1_hscale);
	gui->get_widget("trq_k2_hscale",	trq_k2_hscale);
	gui->get_widget("trq_ke1_hscale",	trq_ke1_hscale);
	gui->get_widget("trq_ke2_hscale",	trq_ke2_hscale);
	gui->get_widget("trq_lim_hscale",	trq_lim_hscale);
	
	gui->get_widget("pos_amp_hscale",	pos_amp_hscale);
	gui->get_widget("pos_freq_hscale",	pos_freq_hscale);
	gui->get_widget("pos_off_hscale",	pos_off_hscale);
	gui->get_widget("pos_k1_hscale",	pos_k1_hscale);
	gui->get_widget("pos_k2_hscale",	pos_k2_hscale);
	gui->get_widget("pos_ke1_hscale",	pos_ke1_hscale);
	gui->get_widget("pos_ke2_hscale",	pos_ke2_hscale);

	gui->get_widget("open_k_hscale",	open_k_hscale);
	gui->get_widget("open_B_hscale",	open_B_hscale);
	gui->get_widget("open_I_hscale",	open_I_hscale);
	gui->get_widget("open_f_hscale",	open_f_hscale);
	gui->get_widget("open_a_hscale",	open_a_hscale);
	gui->get_widget("open_lim_hscale",	open_lim_hscale);
	

	gui->get_widget("drawing_area",		drawing_area);

	gui->get_widget("motor_torque_progress_bar", motor_torque_progress_bar);

	gui->get_widget("log_file_chkbox",	log_file_chkbox);
	gui->get_widget("log_file_name",	log_file_name);

	gui->get_widget("restart_button",	restart_button);
	gui->get_widget("enable_button",	enable_button);
	gui->get_widget("disable_button",	disable_button);

//	gui->get_widget("log_start_toggle", log_start_toggle);

	// Initialize GUI objects
	motor_torque_hscale->set_range(-30.0, 30.0);

	motor_position_hscale->set_range(2.0, 3.5);
	p_motor_position_hscale->set_range(0., 1000.);
	d_motor_position_hscale->set_range(0., 50.);
	
	

	force_hscale->set_range(-20.0, 20.0);
	force_hscale->set_value(0.0);
	pos_hscale->set_range(2.0, 4.0);
	pos_hscale->set_value(3.1);
	tp_hscale->set_range(0.0, 1.0);
	tp_hscale->set_value(0.5);
	p_force_hscale->set_range(0., 100.0);
	d_force_hscale->set_range(0., 10.0);
	f_force_hscale->set_range(0., 1.0);
	p_pos_hscale->set_range(0., 500.0);
	d_pos_hscale->set_range(0., 10.0);
	force_trq_lim_hscale->set_range(0.0, 20.0);
	force_trq_lim_hscale->set_value(20.0);
	
	
	trq_amp_hscale->set_range(0.0, 15.0);
	trq_amp_hscale->set_value(0.0);
	trq_freq_hscale->set_range(0.0, 20.0);
	trq_freq_hscale->set_value(0.0);
	trq_off_hscale->set_range(-15.0, 15.0);
	trq_k1_hscale->set_range(0., 1000.0);
	trq_k1_hscale->set_value(0.0);
	trq_k2_hscale->set_range(0., 100.0);
	trq_k2_hscale->set_value(0.0);
	trq_ke1_hscale->set_range(0.0, 50000.0);
	trq_ke1_hscale->set_value(30000.0);
	trq_ke2_hscale->set_range(0.0, 300.0);
	trq_ke2_hscale->set_value(100.0);
	trq_lim_hscale->set_range(0.0, 20.0);
	trq_lim_hscale->set_value(20.0);
	
	pos_amp_hscale->set_range(0.0, 0.5);
	pos_amp_hscale->set_value(0.0);
	pos_freq_hscale->set_range(0.0, 20.0);
	pos_freq_hscale->set_value(0.0);
	pos_off_hscale->set_range(2.0, 4.0);
	pos_k1_hscale->set_range(0.0, 10000.0);
	pos_k1_hscale->set_value(5000.0);
	pos_k2_hscale->set_range(0.0, 500.0);
	pos_k2_hscale->set_value(100.0);
	pos_ke1_hscale->set_range(0.0, 2000.0);
	pos_ke1_hscale->set_value(1000.0);
	pos_ke2_hscale->set_range(0.0, 20.0);
	pos_ke2_hscale->set_value(5.0);


	open_k_hscale->set_range(0.0, 2000.0);
	open_k_hscale->set_value(1300.0);
	open_B_hscale->set_range(0.0, 2.0);
	open_B_hscale->set_value(1.3);
	open_I_hscale->set_range(0.0, 0.2);
	open_I_hscale->set_value(0.085);
	open_f_hscale->set_range(0., 100.0);
	open_f_hscale->set_value(0.0);
	open_a_hscale->set_range(0., 20.0);
	open_a_hscale->set_value(0.0);
	open_lim_hscale->set_range(0.0, 20.0);
	open_lim_hscale->set_value(20.0);
	
	
	motor_torque_progress_bar->set_fraction(0.);

	// Create the path to the data file with the last state of the gui gains.
	std::string gui_state_file = std::string(argv[0]);
	gui_state_file = gui_state_file.substr(0, gui_state_file.rfind("/bin"));
	gui_state_file = gui_state_file.append("/src/gui_last_state.dat");

	drawing_allocation = drawing_area->get_allocation();

	// Connect buttons to functions.
	log_file_chkbox->signal_toggled().connect( sigc::ptr_fun(log_chkbox_toggled) );
	
	cut_chkbox->signal_toggled().connect( sigc::ptr_fun(cut_chkbox_toggled) );
	
//	log_start_toggle->signal_toggled().connect( sigc::ptr_fun(log_chkbox_toggled) );
	
	restart_button->signal_clicked().connect( sigc::ptr_fun( restart_robot ) );
	enable_button->signal_clicked().connect( sigc::ptr_fun( enable_motors ) );
	disable_button->signal_clicked().connect( sigc::ptr_fun( disable_motors ) );

	controller_notebook->signal_switch_page().connect( sigc::ptr_fun(switch_controllers) );

	sigc::connection conn = Glib::signal_timeout().connect(sigc::ptr_fun(poke_controller), 100); // 50 is the timeout in milliseconds

	gtk.run(*window);

	//*************************************************************************
	// Wait for threads to finish.

	ROS_INFO( "Waiting for threads to finish." );
	logging_flag = 0;
//	ros::spin();
	
	if (datalogging_thread != NULL)
		pthread_join( datalogging_thread, NULL );


	//*************************************************************************
	// send a final command
	
	global_data.command					= CMD_DISABLE;
	global_data.controller_requested	= 0;
	poke_controller();
	
	//*************************************************************************

	// Disconnect from kernel's shm.

	rt_shm_free( nam2num( SHM_NAME ) );

	ROS_INFO("\nDisconnected from SHM.");

	//*************************************************************************

	return 0;
}



//*****************************************************************************

// Log a data entry.

void log_data_entry( FILE * fp, int i )
{
	ControllerInput		*c_in;
	ControllerOutput	*c_out;

	c_in	= &shm->controller_input[i];
	c_out 	= &shm->controller_output[i];

	fprintf( fp, "%f, %u, %u, %f, %f, %f, %f, %f, %u\n",
				c_out->time,
				c_in->motor_cnt,
				c_in->leg_cnt,
				c_in->motor_angle,
				c_in->leg_angle,
				c_in->motor_velocity,
				c_in->leg_velocity,
				c_out->motor_torque,
				c_in->command );
}

//*****************************************************************************
// Datalog while the robot is running.

void * datalogging_task( void * arg )
{
	int i = shm->io_index;
	FILE *log_p = fopen( log_file_name->get_text().c_str(), "w" );
	
	if (log_p == NULL) {
		printf("Failed to open: %s\n",log_file_name->get_text().c_str());
		pthread_exit( NULL );
	}
	
	
	// This print cannot be a ROS_INFO, because of a problem in the ROS header file?
	//ROS_INFO( "Writing %u log entries to %s.", shm->io_index, QUOTEME(LOG_FILENAME) );

	// Create file header.
	fprintf( log_p, "Seconds, Motor Count, Leg Count, Motor Angle, Leg Angle, Motor Velocity, Leg Velocity, Motor Torque, Command\n");

	while ( (logging_flag == 1) )
	{
		if ( i != shm->io_index )
		{
			log_data_entry( log_p, i );
			
			i = (++i) % SHM_TO_USPACE_ENTRIES;
		}

		pthread_yield();
		ros::spinOnce;
	}

	while ( i != shm->io_index )
	{
		log_data_entry( log_p, i );
		
		i = (++i) % SHM_TO_USPACE_ENTRIES;
	}

	fclose(log_p);
	

	pthread_exit( NULL );
}


////////////////////////////////////////////////////////////////////////////////
//		GUI functions

void log_chkbox_toggled( void ) {
//	pthread_t datalogging_thread;

	if ( log_file_chkbox->get_active() ) {

		// open the file and signal the datalogger thread to start logging
//		printf("filename:%s\n\n",log_file_chooser->get_filename().c_str());
		
		if (*(log_file_name->get_text().c_str()) == '\0') {
			log_file_chkbox->set_active(false);
			return;	
		}
		logging_flag = 1;
		pthread_create( &datalogging_thread, NULL, datalogging_task, NULL );
		
//		if (fp == NULL) {
//			printf("Failed to open: %s\n",log_file_chooser->get_filename().c_str());
//			log_file_chkbox->set_active(false);
//			logging_flag = 0;
//		}
//		else {
//			logging_flag = 2;
//		}
		
	}
	else {
		// signal the datalogger to stop
		logging_flag = 0;
	}
}

void cut_chkbox_toggled( void ) {
//	pthread_t datalogging_thread;

	if ( cut_chkbox->get_active() ) {
		cut_flag = 1;
	}
	else {
		cut_flag = 0;
	}
}


void restart_robot( void )
{
	if ( global_data.command == CMD_DISABLE )
	{
		global_data.command					= CMD_RESTART;
		global_data.controller_requested	= NO_CONTROLLER;
		controller_notebook->set_current_page( NO_CONTROLLER );
	}
}

void enable_motors( void )
{
	global_data.command = CMD_RUN;
}

void disable_motors( void )
{
	global_data.command = CMD_DISABLE;
}

void log_start( void )
{

}

void switch_controllers(GtkNotebookPage* page, guint page_num)
{
	if ( global_data.command == CMD_DISABLE )
		global_data.controller_requested = page_num;
	else
		controller_notebook->set_current_page( global_data.controller_requested );
}

bool poke_controller( void )
{
	char buffer[20];
	int i = 0;

	// get the slider values
	switch (global_data.controller_requested)
	{
		case NO_CONTROLLER:
			//MTR_TRQ_CONTROLLER_DATA(&(fca_srv.request.control_data.elems))->mtr_trqA = 0.;
			break;
		case MOTOR_TORQUE_CONTROLLER:
			((MtrTrqControllerData *)(&(global_data.data)))->mtr_trq = motor_torque_hscale->get_value();
			break;
		case MOTOR_POSITION_CONTROLLER:
			((MtrPosControllerData *)(&(global_data.data)))->mtr_ang			= motor_position_hscale->get_value();
			((MtrPosControllerData *)(&(global_data.data)))->p_gain				= p_motor_position_hscale->get_value();
			((MtrPosControllerData *)(&(global_data.data)))->d_gain				= d_motor_position_hscale->get_value();
			break;
		case FORCE_CONTROLLER:
			((ForceControllerData *)(&(global_data.data)))->torque_cmd			= force_hscale->get_value();
			((ForceControllerData *)(&(global_data.data)))->pos_cmd				= pos_hscale->get_value();
			((ForceControllerData *)(&(global_data.data)))->tp_ratio			= tp_hscale->get_value();
			((ForceControllerData *)(&(global_data.data)))->trq_lim				= force_trq_lim_hscale->get_value();
			if (cut_flag == 0) {
				((ForceControllerData *)(&(global_data.data)))->torque_p_gain	= p_force_hscale->get_value();
				((ForceControllerData *)(&(global_data.data)))->torque_d_gain	= d_force_hscale->get_value();
				((ForceControllerData *)(&(global_data.data)))->torque_f_gain	= f_force_hscale->get_value();
				((ForceControllerData *)(&(global_data.data)))->pos_p_gain		= p_pos_hscale->get_value();
				((ForceControllerData *)(&(global_data.data)))->pos_d_gain		= d_pos_hscale->get_value();
			}
			else {
				((ForceControllerData *)(&(global_data.data)))->torque_p_gain	= 0;
				((ForceControllerData *)(&(global_data.data)))->torque_d_gain	= 0;
				((ForceControllerData *)(&(global_data.data)))->torque_f_gain	= 0;
				((ForceControllerData *)(&(global_data.data)))->pos_p_gain		= 0;
				((ForceControllerData *)(&(global_data.data)))->pos_d_gain		= 0;
			}
			
			break;
			
		case TORQUE_SINE_CONTROLLER:
			((TorqueSineControllerData *)(&(global_data.data)))->trq_amp		= trq_amp_hscale->get_value();
			((TorqueSineControllerData *)(&(global_data.data)))->trq_freq		= 2*PI*trq_freq_hscale->get_value();
			((TorqueSineControllerData *)(&(global_data.data)))->trq_off		= trq_off_hscale->get_value();
			((TorqueSineControllerData *)(&(global_data.data)))->trq_k1			= trq_k1_hscale->get_value();
			((TorqueSineControllerData *)(&(global_data.data)))->trq_k2			= trq_k2_hscale->get_value();
			((TorqueSineControllerData *)(&(global_data.data)))->trq_ke1		= trq_ke1_hscale->get_value();
			((TorqueSineControllerData *)(&(global_data.data)))->trq_ke2		= trq_ke2_hscale->get_value();
			((TorqueSineControllerData *)(&(global_data.data)))->trq_lim		= trq_lim_hscale->get_value();
			break;
		case POSITION_SINE_CONTROLLER:
			((PositionSineControllerData *)(&(global_data.data)))->pos_amp		= pos_amp_hscale->get_value();
			((PositionSineControllerData *)(&(global_data.data)))->pos_freq		= pos_freq_hscale->get_value();
			((PositionSineControllerData *)(&(global_data.data)))->pos_off		= pos_off_hscale->get_value();
			((PositionSineControllerData *)(&(global_data.data)))->pos_k1		= pos_k1_hscale->get_value();
			((PositionSineControllerData *)(&(global_data.data)))->pos_k2		= pos_k2_hscale->get_value();
			((PositionSineControllerData *)(&(global_data.data)))->pos_ke1		= pos_ke1_hscale->get_value();
			((PositionSineControllerData *)(&(global_data.data)))->pos_ke2		= pos_ke2_hscale->get_value();
			break;
		case OPENLOOP_SINE_CONTROLLER:
			((OpenloopSineControllerData *)(&(global_data.data)))->open_k		= open_k_hscale->get_value();
			((OpenloopSineControllerData *)(&(global_data.data)))->open_B		= open_B_hscale->get_value();
			((OpenloopSineControllerData *)(&(global_data.data)))->open_I		= open_I_hscale->get_value();
			((OpenloopSineControllerData *)(&(global_data.data)))->open_f		= open_f_hscale->get_value();
			((OpenloopSineControllerData *)(&(global_data.data)))->open_a		= open_a_hscale->get_value();
			((OpenloopSineControllerData *)(&(global_data.data)))->open_lim		= open_lim_hscale->get_value();
			break;
	}


//	printf("gd.cmd %u\n",global_data.command);

	// Load request into the kernel buffer.
	shm->controller_data[1 - shm->control_index].command				= global_data.command;
	shm->controller_data[1 - shm->control_index].controller_requested	= global_data.controller_requested;

	for (i = 0; i < SIZE_OF_CONTROLLER_DATA; i++)
		shm->controller_data[1 - shm->control_index].data[i] = global_data.data[i];
	
	// signal the kernel
	shm->req_switch		= true;
	while (shm->req_switch == true) {											// wait until the kernel acks
		usleep(100);	
	}


	// update the cartoon
	draw_leg();

	
	if ( global_data.command == CMD_RESTART )
		global_data.command = CMD_DISABLE;

	// Move the sliders if the controller is disabled.
	if ( global_data.command == CMD_DISABLE )
	{
		motor_position_hscale->set_value(shm->controller_input[shm->io_index - 1].motor_angle);
		pos_hscale->set_value(shm->controller_input[shm->io_index - 1].motor_angle);
		trq_off_hscale->set_value(0);
		pos_off_hscale->set_value(shm->controller_input[shm->io_index - 1].motor_angle);
	}

	// Update the motor torque progress bars.
	motor_torque_progress_bar->set_fraction( MIN ( ABS(shm->controller_output[shm->io_index - 1].motor_torque), MTR_MAX_TRQ ) / MTR_MAX_TRQ);

	
	pthread_yield();
	
	return true;
}

void draw_leg()
{

	int width = drawing_area->get_width();
    int height = drawing_area->get_height();
    int lesser = MIN(width, height);
	
	float segment_length = lesser/2;
	float motor_radius = lesser/2;

	float start_x = width/2;
	float start_y = height/2;

	drawing_area->get_window()->clear();

	cr = drawing_area->get_window()->create_cairo_context();

	cr->set_line_width(12.0);
	

	
	if (shm->controller_input[shm->io_index - 1].command == CMD_DISABLE) {
		cr->set_source_rgb(0.1, 0.1, 0.1);
//		cr->arc(start_x, start_y, segment_length);
		
		cr->save();
	    cr->arc(start_x, start_y, lesser/2, 0.0, 2.0 * M_PI); // full circle
	    cr->set_source_rgba(0.8, 0.0, 0.0, 0.6);    // partially translucent
	    cr->fill_preserve();
	    
	    cr->restore();  // back

//		cr->move_to(start_x, start_y);
//		cr->rel_line_to(segment_length*cos(PI/4) , segment_length*sin(PI/4));
//		cr->move_to(start_x, start_y);
//		cr->rel_line_to(segment_length*cos(PI+PI/4) , segment_length*sin(PI+PI/4));

		cr->stroke();
	}

	// Draw the motors
	cr->set_source_rgb(0.0, 0.8, 0.0);
	// A
	cr->move_to(start_x, start_y);
	cr->rel_line_to(motor_radius * cos(shm->controller_input[shm->io_index - 1].motor_angle), -motor_radius * sin(shm->controller_input[shm->io_index - 1].motor_angle));
//	cr->move_to(start_x, start_y);
//	cr->rel_line_to(-motor_radius * cos(shm->controller_input[shm->io_index - 1].motor_angle), motor_radius * sin(shm->controller_input[shm->io_index - 1].motor_angle));

	cr->stroke();

	// Draw the leg
	cr->move_to(start_x, start_y);
	// OSU orange 216, 90, 26
	cr->set_source_rgb(0.4, 0.4, 0.4);
	//cr->set_source_rgb(0.8, 0.0, 0.0);
	// A
	cr->rel_line_to(segment_length * cos(shm->controller_input[shm->io_index - 1].leg_angle), -segment_length * sin(shm->controller_input[shm->io_index - 1].leg_angle));

	cr->stroke();
}






